clear;clc
close all;
load('aerodata.mat');
if ~isstruct(pp) || ~isfield(pp, 'form')
    error('pp 不是有效的样条插值结构！');
end
%[breaks, coefs] = unmkpp(pp); % 尝试提取数据
%pp = mkpp(breaks, coefs);     % 重新创建 pp 结构体

% 仿真参数
x0 = [0,0,0,15*cosd(45),15*sind(45),0,my_angle2quat([deg2rad(45);deg2rad(0);0])',0,0,0];
%stop_cond = @(t, y) y(2,1) < 0.5 && t>1;
options = odeset('Events',@stop_cond);
[t,y]=ode45(@(t,x) PN3_ode(t,x,pp),[0 100],x0,options);

y_ = y.';
t_ = t.';
plot_num = 1;
angle_t = my_quat2angle(y_(7:10,:));
att_r = zeros(3,size(angle_t,2));
% 获取飞行器姿态
for i = 1:1:size(angle_t,2)
phi = deg2rad(angle_t(1,i));psi=deg2rad(angle_t(2,i));gamma=deg2rad(angle_t(3,i));
C_BI = [cos(phi) * cos(psi), sin(phi)* cos(psi), -sin(psi);
cos(phi)* sin(psi)* sin(gamma) - sin(phi) * cos(gamma), sin(phi)* sin(psi)* sin(gamma) + cos(phi) * cos(gamma), cos(psi)* sin(gamma);
cos(phi)* sin(psi)* cos(gamma) + sin(phi) * sin(gamma), sin(phi)* sin(psi)* cos(gamma) - cos(phi) * sin(gamma), cos(psi)* cos(gamma)];
att_r(:,i) = C_BI.' * [1;0;0];
end
figure(plot_num);plot_num = plot_num + 1;
plot3(y_(1,:),-y_(3,:),y_(2,:),'r');
hold on;
grid on;
xlabel('x');
ylabel('y');
zlabel('z');

selected_y_ = y_(:,5:5:end);
selected_att_r = att_r(:,5:5:end);
figure(plot_num);plot_num = plot_num + 1;
%plot3(y_(1,:),-y_(3,:),y_(2,:),'r');
scatter3(selected_y_(1,:), -selected_y_(3,:), selected_y_(2,:), 50, 'filled');
hold on;
quiver3(selected_y_(1,:), -selected_y_(3,:), selected_y_(2,:), selected_att_r(1,:), -selected_att_r(3,:), selected_att_r(2,:),...
    0.1, 'r', 'LineWidth', 1, 'MaxHeadSize', 0.00001);
grid on;
axis equal;
xlabel('x');
ylabel('y');
zlabel('z');

figure(plot_num);plot_num = plot_num + 1;
plot(t_, y_(1,:));
title("x---t");


figure(plot_num);plot_num = plot_num + 1;
plot(t_, y_(2,:));
title("y---t");

figure(plot_num);plot_num = plot_num + 1;
plot(t_, y_(3,:));
title("z---t");

function [value, isterminal, direction] = stop_cond(t, y)
    value = y(2) - 0.5;  % y(2) 表示高度，触地条件 y(2) == 0.5
    isterminal = 1;  % 终止仿真
    direction = -1;  % 只检测高度从上往下（正到负）穿过 0.5
end